Advertisement
Guest User

Untitled

a guest
Aug 2nd, 2022
38
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.88 KB | None | 0 0
  1. ; Helper macro for containing functionality needed to
  2. ; safely perform a G38.3 move.
  3.  
  4. o<_psng2_probe> sub
  5.  
  6. ; Save and autorestore model states
  7. M73
  8.  
  9. ; Parameters for this macro
  10. #<search_distance> = #1
  11. #<latch_distance> = #2
  12. ; 0 = +X, 90 = +Y, 180 = -X, 270 = -Y
  13. #<search_xy_angle> = #3
  14. ; 0 = XY, 90 = Z
  15. #<search_z_angle> = #4
  16. #<searchvel> = #5
  17. #<probevel> = #6
  18. #<probesettle> = #7
  19.  
  20.  
  21. ; check inputs
  22. O10 if [#<search_distance> LE 0]
  23. O10 return [-1]
  24. O10 endif
  25. O11 if [[#<search_z_angle> LT 0] OR [#<search_z_angle> GT 90]]
  26. O11 return [-2]
  27. O11 endif
  28. O12 if [#<searchvel> LE 0]
  29. O12 return [-3]
  30. O12 endif
  31. O13 if [#<probevel> LE 0]
  32. O13 return [-4]
  33. O13 endif
  34. O14 if [#<probesettle> LT 0]
  35. O14 return [-12]
  36. O14 endif
  37.  
  38. #<x_f> = [COS[#<search_xy_angle>] * COS[#<search_z_angle>]]
  39. #<y_f> = [SIN[#<search_xy_angle>] * COS[#<search_z_angle>]]
  40. ; always be probing down
  41. #<z_f> = [-SIN[#<search_z_angle>]]
  42.  
  43. ; Set incremental mode
  44. G91
  45.  
  46. ;check for error
  47. ;probe error
  48. M66 P0 L0
  49. O20 if [#5399 EQ 1]
  50. O20 return [-5]
  51. O20 endif
  52. ;low battery
  53. M66 P1 L0
  54. O21 if [#5399 EQ 1]
  55. O21 return [-6]
  56. O21 endif
  57.  
  58. ; Check if probe is already tripped
  59. O22 if [#<_hal[motion.probe-input]> EQ 1]
  60. O22 return [-7]
  61. O22 endif
  62.  
  63. ; initial position
  64. #<x> = #<_x>
  65. #<y> = #<_y>
  66. #<z> = #<_z>
  67. ; Perform the Search
  68. G38.3 X[#<search_distance> * #<x_f>] Y[#<search_distance> * #<y_f>] Z[#<search_distance> * #<z_f>] F[#<searchvel>]
  69.  
  70. ; Check to see if we failed to make contact
  71. O30 if [#5070 EQ 0]
  72. O30 return [-8]
  73. O30 endif
  74.  
  75. ;probe error
  76. M66 P0 L0
  77. O31 if [#5399 EQ 1]
  78. O31 return [-5]
  79. O31 endif
  80.  
  81. ; check how far we moved. If less that latch_distance then go back to where we started instead of the latch_distance
  82. #<dx> = [#<x> - #<_x>]
  83. #<dy> = [#<y> - #<_y>]
  84. #<dz> = [#<z> - #<_z>]
  85. #<dist_moved> = [SQRT[[#<dx> * #<dx>] + [#<dy> * #<dy>] + [#<dz> * #<dz>]]]
  86.  
  87. O40 if [#<dist_moved> LT #<latch_distance>]
  88. #<latch_dist> = #<dist_moved>
  89. O40 else
  90. #<latch_dist> = #<latch_distance>
  91. O40 endif
  92.  
  93. ; it is safe to do a G1 move here as we already move through this space without issue
  94. G1 X[-#<latch_dist> * #<x_f>] Y[-#<latch_dist> * #<y_f>] Z[-#<latch_dist> * #<z_f>]
  95.  
  96. ; let the probe settle
  97. G4 P[#<probesettle>]
  98.  
  99. M66 E0 L0
  100. ; Should never happen but good practice
  101. O50 if [#<_hal[motion.probe-input]> EQ 1]
  102. O50 return [-7]
  103. O50 endif
  104.  
  105. ; fine probe
  106. G38.3 X[#<latch_dist> * 2 * #<x_f>] Y[#<latch_dist> * 2 * #<y_f>] Z[#<latch_dist> * 2 * #<z_f>] F[#<probevel>]
  107.  
  108. ; Check to see if we failed to make contact
  109. O60 if [#5070 EQ 0]
  110. O60 return [-8]
  111. O60 endif
  112.  
  113. ;probe error
  114. M66 P0 L0
  115. O61 if [#5399 EQ 1]
  116. O61 return [-5]
  117. O61 endif
  118.  
  119. ; Move away
  120. G1 X[-#<latch_dist> * #<x_f>] Y[-#<latch_dist> * #<y_f>] Z[-#<latch_dist> * #<z_f>] F[#<searchvel>]
  121.  
  122. ; let the probe settle
  123. G4 P[#<probesettle>]
  124.  
  125. o<_psng2_probe> endsub [0]
  126. M2
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement